#include "Mcs.h"
#include "../Yogurt/utility.h"

Mcs::Mcs()
{   
    filePointer = openSerialPort();
    leftID = 0x07;
    rightID = 0x09;
    Turning_speed_45 = 175;
    Turning_speed = 200;
    Back_turning_speed = 1;
    LeftFrontSpeed = 0;
    LeftRearSpeed = 0;
    RightFrontSpeed = 0;
    RightRearSpeed = 0;
	time(&start);
	time(&rightnow);
    diff = 0;
}

// needs to be called before using
void Mcs::begin()
{
    sendByte(INITIALPACKET);
}

void Mcs::configMCs()
{
    int in1;

    cout << "Enter value to change:" << endl;
    cout << "0: Device ID" << endl;
    cout << "1: PWM Parameter" << endl;
    cout << "2: Get Device ID" << endl;
    cin >> in1;

    switch(int(in1))
    {
		case CONFIG_DEVICEID:
			while(1)
			{
				cout << "Enter DeviceID value (0-127); only 1 MC should be connected!" << endl;
				cin >> in1;

				if (in1 >= 0 && in1 <= 127)
				{
					sendByte(0xAA);
					sendByte(0x84);
					sendByte(0);
					sendByte(in1);
					sendByte(0x55);
					sendByte(0x2A);
					break;
				}
				else
				{
					cout << "Error, only use 0-127" << endl;
				}
			}
		break;
		case CONFIG_PWM:
			cout << "Enter Pulse Width Modulation value" << endl;
			cout << "0x8- is 8 bit" << endl;
			cout << "0x7- is 7 bit" << endl;
			cin >> in1;

			if(in1 == 7)
			{
				//Individual Connection:
				sendByte(0x84);
				sendByte(1);
				sendByte(0);
				sendByte(0x55);
				sendByte(0x2A);

				//Dual Connecation (doesn't seem to work)
				//Set Left MC
//				sendByte(0xAA);
//				sendByte(leftID);
//				sendByte(0x04);
//				sendByte(1);
//				sendByte(0);
//				sendByte(0x55);
//				sendByte(0x2A);
//				//Set Right MC
//				sendByte(0xAA);
//				sendByte(rightID);
//				sendByte(0x04);
//				sendByte(1);
//				sendByte(0);
//				sendByte(0x55);
//				sendByte(0x2A);
			}
			else
			{
				//Individual Connection:
				sendByte(0x84);
				sendByte(1);
				sendByte(1);
				sendByte(0x55);
				sendByte(0x2A);

				//Set Left MC
//				sendByte(0xAA);
//				sendByte(leftID);
//				sendByte(0x04);
//				sendByte(1);
//				sendByte(1);
//				sendByte(0x55);
//				sendByte(0x2A);
//				//Set Right MC
//				sendByte(0xAA);
//				sendByte(rightID);
//				sendByte(0x04);
//				sendByte(1);
//				sendByte(1);
//				sendByte(0x55);
//				sendByte(0x2A);
			}
		break;
		case 2:
			sendByte(GETCONFIG);
		    sendByte(CONFIG_DEVICEID);
		    WaitSeconds(1);
		    readByte();
			break;
		default:
			cout << "You entered a wrong value" << endl;
		break;
    }
}

void Mcs::MoveForwardAtSpeed(uint8_t speed)
{
    MoveForward((uint8_t)speed,(uint8_t)speed,(uint8_t)speed,(uint8_t)speed);
}

void Mcs::MoveBackwardAtSpeed(uint8_t speed)
{
    int n;
    unsigned char m[16];
    m[0] = INITIALPACKET;
    m[4] = INITIALPACKET;
    m[8] = INITIALPACKET;
    m[12] = INITIALPACKET;
    m[1] = leftID;
    m[5] = leftID;
    m[9] = rightID;
    m[13] = rightID;

    if(speed > 127)
    {
		m[2] = MOTOR0REVERSEFASTPACKET;
		m[6] = MOTOR1REVERSEFASTPACKET;
		m[10] = MOTOR0REVERSEFASTPACKET;
		m[14] = MOTOR1REVERSEFASTPACKET;
		m[3] = speed-127;
		m[7] = speed-127;
		m[11] = speed-127;
		m[15] = speed-127;
	    LeftFrontSpeed = speed-127;
	    LeftRearSpeed = speed-127;
	    RightFrontSpeed = speed-127;
	    RightRearSpeed = speed-127;
    }
    else
    {
		m[2] = MOTOR0REVERSEPACKET;
		m[6] = MOTOR1REVERSEPACKET;
		m[10] = MOTOR0REVERSEPACKET;
		m[14] = MOTOR1REVERSEPACKET;
		m[3] = speed;
		m[7] = speed;
		m[11] = speed;
		m[15] = speed;
	    LeftFrontSpeed = speed;
	    LeftRearSpeed = speed;
	    RightFrontSpeed = speed;
	    RightRearSpeed = speed;
    }

    //fprintf(stderr, "Sending Byte: %x\n", m[0]);
#if !SIM
    n=write(filePointer,m,16);
    if (n<0)
    	Error("Unable to write");
#endif
}

void Mcs::Stop()
{
	MoveForward(0,0,0,0);
}

void Mcs::AdjustToStayOnLine(int degrees,int distOff)
{
	//+ = driving/off to the left, - = driving/off to the right
	int turn = degrees + distOff/2;

	cout << "Adjusting by " << endl;
	cout << turn << endl;
	if(turn>1)
	{
		MoveForward(254,254,254-turn,254-turn);
	}
	else
		if(turn<1)
			MoveForward(254+turn,254+turn,254,254);
}

void Mcs::MoveForward(uint8_t L0Speed, uint8_t L1Speed, uint8_t R0Speed, uint8_t R1Speed)
{
	int n;
	unsigned char m[16];
	m[0] = INITIALPACKET;
	m[4] = INITIALPACKET;
	m[8] = INITIALPACKET;
	m[12] = INITIALPACKET;
	m[1] = leftID;
	m[5] = leftID;
	m[9] = rightID;
	m[13] = rightID;

	if(L0Speed > 127)
	{
		m[2] = MOTOR0FORWARDFASTPACKET;
		m[3] = L0Speed-127;
	}
	else
	{
		m[2] = MOTOR0FORWARDPACKET;
		m[3] = L0Speed;
	}

	if(L1Speed > 127)
	{
		m[6] = MOTOR1FORWARDFASTPACKET;
		m[7] = L1Speed-127;
	}
	else
	{
		m[6] = MOTOR1FORWARDPACKET;
		m[7] = L1Speed;
	}

	if(R0Speed > 127)
	{
		m[10] = MOTOR0FORWARDFASTPACKET;
		m[11] = R0Speed-127;
	}
	else
	{
		m[10] = MOTOR0FORWARDPACKET;
		m[11] = R0Speed;
	}

	if(R1Speed > 127)
	{
		m[14] = MOTOR1FORWARDFASTPACKET;
		m[15] = R1Speed-127;
	}
	else
	{
		m[14] = MOTOR1FORWARDPACKET;
		m[15] = R1Speed;
	}

//    for (int a = 0; a<16;a++)
//    {
//    	fprintf(stdout, "%x\n",m[a]);
//    }
#if !SIM
	n=write(filePointer,m,16);
	if (n<0)
		Error("Unable to write");
#endif
}

void Mcs::motor0Forward(uint8_t speed)
{
    if ( speed > 127 )
    {
    	sendByte(INITIALPACKET);
    	sendByte(leftID);
        sendByte(MOTOR0FORWARDFASTPACKET);
        sendByte(speed-127);
    }
    else
    {
    	sendByte(INITIALPACKET);
    	sendByte(leftID);
        sendByte(MOTOR0FORWARDPACKET);
        sendByte(speed);
    }
}

void Mcs::motor1Forward(uint8_t speed)
{
    if ( speed > 127 )
    {
    	sendByte(INITIALPACKET);
    	sendByte(leftID);
        sendByte(MOTOR1FORWARDFASTPACKET);
        sendByte(speed-127);
    }
    else
    {
    	sendByte(INITIALPACKET);
    	sendByte(leftID);
        sendByte(MOTOR1FORWARDPACKET);
        sendByte(speed);
    }
}

void Mcs::motor2Forward(uint8_t speed)
{
    if ( speed > 127 )
    {
    	sendByte(INITIALPACKET);
    	sendByte(rightID);
        sendByte(MOTOR0FORWARDFASTPACKET);
        sendByte(speed-127);
    }
    else
    {
    	sendByte(INITIALPACKET);
    	sendByte(rightID);
        sendByte(MOTOR0FORWARDPACKET);
        sendByte(speed);
    }
}

void Mcs::motor3Forward(uint8_t speed)
{
    if ( speed > 127 )
    {
    	sendByte(INITIALPACKET);
    	sendByte(rightID);
        sendByte(MOTOR1FORWARDFASTPACKET);
        sendByte(speed-127);
    }
    else
    {
    	sendByte(INITIALPACKET);
    	sendByte(rightID);
        sendByte(MOTOR1FORWARDPACKET);
        sendByte(speed);
    }
}

void Mcs::motor0Reverse(uint8_t speed)
{
    if ( speed > 127 )
    {
    	sendByte(INITIALPACKET);
    	sendByte(leftID);
        sendByte(MOTOR0REVERSEFASTPACKET);
        sendByte(speed-127);
    }
    else
    {
    	sendByte(INITIALPACKET);
    	sendByte(leftID);
        sendByte(MOTOR0REVERSEPACKET);
        sendByte(speed);
    }
}

void Mcs::motor1Reverse(uint8_t speed)
{
    if ( speed > 127 )
    {
    	sendByte(INITIALPACKET);
    	sendByte(leftID);
        sendByte(MOTOR1REVERSEFASTPACKET);
        sendByte(speed-127);
    }
    else
    {
    	sendByte(INITIALPACKET);
    	sendByte(leftID);
        sendByte(MOTOR1REVERSEPACKET);
        sendByte(speed);
    }
}

void Mcs::motor2Reverse(uint8_t speed)
{
    if ( speed > 127 )
    {
    	sendByte(INITIALPACKET);
    	sendByte(rightID);
        sendByte(MOTOR0REVERSEFASTPACKET);
        sendByte(speed-127);
    }
    else
    {
    	sendByte(INITIALPACKET);
    	sendByte(rightID);
        sendByte(MOTOR0REVERSEPACKET);
        sendByte(speed);
    }
}

void Mcs::motor3Reverse(uint8_t speed)
{
    if ( speed > 127 )
    {
    	sendByte(INITIALPACKET);
    	sendByte(rightID);
        sendByte(MOTOR1REVERSEFASTPACKET);
        sendByte(speed-127);
    }
    else
    {
    	sendByte(INITIALPACKET);
    	sendByte(rightID);
        sendByte(MOTOR1REVERSEPACKET);
        sendByte(speed);
    }
}

void Mcs::motor0Coast()
{
	sendByte(INITIALPACKET);
	sendByte(leftID);
    sendByte(MOTOR0COASTPACKET);
}

void Mcs::motor1Coast()
{
	sendByte(INITIALPACKET);
	sendByte(leftID);
    sendByte(MOTOR1COASTPACKET);
}

void Mcs::motor2Coast()
{
	sendByte(INITIALPACKET);
	sendByte(rightID);
    sendByte(MOTOR0COASTPACKET);
}

void Mcs::motor3Coast()
{
	sendByte(INITIALPACKET);
	sendByte(rightID);
    sendByte(MOTOR1COASTPACKET);
}

//Turning stuff left motor = 0 & right motor = 1
int Mcs::TurnLeft(uint8_t degrees)
{
    int outValue;
    unsigned char m[16];
    //Stop so that this is consistent
	Stop();
	wait(200);

	switch(degrees)
	{
	case 45:
		//Set Turn commands
	    m[0] = INITIALPACKET;
	    m[4] = INITIALPACKET;
	    m[8] = INITIALPACKET;
	    m[12] = INITIALPACKET;
	    m[1] = leftID;
	    m[5] = leftID;
	    m[9] = rightID;
	    m[13] = rightID;

	    if(Back_turning_speed > 127)
	    {
			m[6] = MOTOR1FORWARDFASTPACKET;
			m[7] = Back_turning_speed;
	    }
	    else
	    {
			m[6] = MOTOR1FORWARDPACKET;
			m[7] = Back_turning_speed;
	    }
		if(Turning_speed_45 > 127)
		{
			m[2] = MOTOR0REVERSEFASTPACKET;
			m[10] = MOTOR0FORWARDFASTPACKET;
			m[14] = MOTOR1FORWARDFASTPACKET;
			m[3] = Turning_speed_45-127;
			m[11] = Turning_speed_45-127;
			m[15] = Turning_speed_45-127;
		}
		else
		{
			m[2] = MOTOR0REVERSEPACKET;
			m[10] = MOTOR0FORWARDPACKET;
			m[14] = MOTOR1FORWARDPACKET;
			m[3] = Turning_speed_45;
			m[11] = Turning_speed_45;
			m[15] = Turning_speed_45;
		}

		//Execute command from switch
		outValue=write(filePointer,m,16);
	    if (outValue<0)
	    	Error("Unable to write");

	    //Allow it to turn
	    WaitSeconds(2);
		break;
	case 90:
		//Set Turn commands
	    m[0] = INITIALPACKET;
	    m[4] = INITIALPACKET;
	    m[8] = INITIALPACKET;
	    m[12] = INITIALPACKET;
	    m[1] = leftID;
	    m[5] = leftID;
	    m[9] = rightID;
	    m[13] = rightID;

	    if(Back_turning_speed > 127)
	    {
			m[6] = MOTOR1FORWARDFASTPACKET;
			m[7] = Back_turning_speed;
	    }
	    else
	    {
			m[6] = MOTOR1FORWARDPACKET;
			m[7] = Back_turning_speed;
	    }
		if(Turning_speed > 127)
		{
			m[2] = MOTOR0REVERSEFASTPACKET;
			m[10] = MOTOR0FORWARDFASTPACKET;
			m[14] = MOTOR1FORWARDFASTPACKET;
			m[3] = Turning_speed-127;
			m[11] = Turning_speed-127;
			m[15] = Turning_speed-127;
		}
		else
		{
			m[2] = MOTOR0REVERSEPACKET;
			m[10] = MOTOR0FORWARDPACKET;
			m[14] = MOTOR1FORWARDPACKET;
			m[3] = Turning_speed;
			m[11] = Turning_speed;
			m[15] = Turning_speed;
		}

		//Execute command from switch
		outValue=write(filePointer,m,16);
	    if (outValue<0)
	    	Error("Unable to write");

	    //Allow it to turn
	    WaitSeconds(3);
		break;
	case 180:
		//Back up a little bit first before turning
		MoveBackwardAtSpeed(50);
		WaitSeconds(1);
		Stop();
		WaitSeconds(1);

		//Reverse backwards and turn
		//Set Turn commands
		m[0] = INITIALPACKET;
		m[4] = INITIALPACKET;
		m[8] = INITIALPACKET;
		m[12] = INITIALPACKET;
		m[1] = leftID;
		m[5] = leftID;
		m[9] = rightID;
		m[13] = rightID;

		if(Back_turning_speed > 127)
		{
			m[2] = MOTOR1FORWARDFASTPACKET;
			m[3] = Back_turning_speed;
		}
		else
		{
			m[2] = MOTOR1FORWARDPACKET;
			m[3] = Back_turning_speed;
		}
		if(Turning_speed > 127)
		{
			m[14] = MOTOR1REVERSEFASTPACKET;
			m[10] = MOTOR0REVERSEFASTPACKET;
			m[6] = MOTOR0FORWARDFASTPACKET;
			m[15] = Turning_speed-127;
			m[11] = Turning_speed-127;
			m[7] = Turning_speed-127;
		}
		else
		{
			m[14] = MOTOR1REVERSEPACKET;
			m[10] = MOTOR0REVERSEPACKET;
			m[6] = MOTOR0FORWARDPACKET;
			m[15] = Turning_speed;
			m[11] = Turning_speed;
			m[7] = Turning_speed;
		}

		//Execute command from switch
		outValue=write(filePointer,m,16);
		if (outValue<0)
			Error("Unable to write");

		//Allow it to turn
		WaitSeconds(1);
		Stop();

		//Turn Right 45
		TurnRight(45);
		WaitSeconds(1);

		//Reverse backwards and turn
		//Set Turn commands
		m[0] = INITIALPACKET;
		m[4] = INITIALPACKET;
		m[8] = INITIALPACKET;
		m[12] = INITIALPACKET;
		m[1] = leftID;
		m[5] = leftID;
		m[9] = rightID;
		m[13] = rightID;

		if(Back_turning_speed > 127)
		{
			m[2] = MOTOR1FORWARDFASTPACKET;
			m[3] = Back_turning_speed;
		}
		else
		{
			m[2] = MOTOR1FORWARDPACKET;
			m[3] = Back_turning_speed;
		}
		if(Turning_speed > 127)
		{
			m[14] = MOTOR1REVERSEFASTPACKET;
			m[10] = MOTOR0REVERSEFASTPACKET;
			m[6] = MOTOR0FORWARDFASTPACKET;
			m[15] = Turning_speed-127;
			m[11] = Turning_speed-127;
			m[7] = Turning_speed-127;
		}
		else
		{
			m[14] = MOTOR1REVERSEPACKET;
			m[10] = MOTOR0REVERSEPACKET;
			m[6] = MOTOR0FORWARDPACKET;
			m[15] = Turning_speed;
			m[11] = Turning_speed;
			m[7] = Turning_speed;
		}

		//Execute command from switch
		outValue=write(filePointer,m,16);
		if (outValue<0)
			Error("Unable to write");

		//Allow it to turn
		WaitSeconds(1);
		Stop();

		//Turn Right 45
		TurnRight(45);
		WaitSeconds(1);
		break;
	default:
		cout << "Error, please enter only 45, 90, or 180" << endl;
		return -1;
	}

	//Stop Turning
	Stop();
	return 0;
}

int Mcs::TurnRight(uint8_t degrees)
{
    int outValue;
    unsigned char m[16];
    //Stop so that this is consistent
	Stop();
	wait(200);

	switch(degrees)
	{
	case 45:
		//Set Turn commands
	    m[0] = INITIALPACKET;
	    m[4] = INITIALPACKET;
	    m[8] = INITIALPACKET;
	    m[12] = INITIALPACKET;
	    m[1] = leftID;
	    m[5] = leftID;
	    m[9] = rightID;
	    m[13] = rightID;

	    if(Back_turning_speed > 127)
	    {
			m[10] = MOTOR0FORWARDFASTPACKET;
			m[11] = Back_turning_speed;
	    }
	    else
	    {
			m[10] = MOTOR0FORWARDPACKET;
			m[11] = Back_turning_speed;
	    }
		if(Turning_speed_45 > 127)
		{
			m[14] = MOTOR1REVERSEFASTPACKET;
			m[2] = MOTOR0FORWARDFASTPACKET;
			m[6] = MOTOR1FORWARDFASTPACKET;
			m[15] = Turning_speed_45-127;
			m[3] = Turning_speed_45-127;
			m[7] = Turning_speed_45-127;
		}
		else
		{
			m[14] = MOTOR1REVERSEPACKET;
			m[2] = MOTOR0FORWARDPACKET;
			m[6] = MOTOR1FORWARDPACKET;
			m[15] = Turning_speed_45-127;
			m[3] = Turning_speed_45-127;
			m[7] = Turning_speed_45-127;
		}

		//Execute command from switch
		outValue=write(filePointer,m,16);
	    if (outValue<0)
	    	Error("Unable to write");

	    //Allow it to turn
	    WaitSeconds(2);
		break;
	case 90:
		//Set Turn commands
	    m[0] = INITIALPACKET;
	    m[4] = INITIALPACKET;
	    m[8] = INITIALPACKET;
	    m[12] = INITIALPACKET;
	    m[1] = leftID;
	    m[5] = leftID;
	    m[9] = rightID;
	    m[13] = rightID;

	    if(Back_turning_speed > 127)
	    {
			m[10] = MOTOR0FORWARDFASTPACKET;
			m[11] = Back_turning_speed;
	    }
	    else
	    {
			m[10] = MOTOR0FORWARDPACKET;
			m[11] = Back_turning_speed;
	    }
		if(Turning_speed > 127)
		{
			m[14] = MOTOR1REVERSEFASTPACKET;
			m[2] = MOTOR0FORWARDFASTPACKET;
			m[6] = MOTOR1FORWARDFASTPACKET;
			m[15] = Turning_speed-127;
			m[3] = Turning_speed-127;
			m[7] = Turning_speed-127;
		}
		else
		{
			m[14] = MOTOR1REVERSEPACKET;
			m[2] = MOTOR0FORWARDPACKET;
			m[6] = MOTOR1FORWARDPACKET;
			m[15] = Turning_speed;
			m[3] = Turning_speed;
			m[7] = Turning_speed;
		}

		//Execute command from switch
		outValue=write(filePointer,m,16);
	    if (outValue<0)
	    	Error("Unable to write");

	    //Allow it to turn
	    WaitSeconds(3);
		break;
	case 180:

		break;
	default:
		cout << "Error, please enter only 45, 90, or 180" << endl;
		return -1;
	}

	//Stop Turning
	Stop();
	return 0;
}

//Getter & Setter Methods

void Mcs::setTurningSpeed(int a){
    Turning_speed = a;
}

int Mcs::getTurningSpeed(){
    return Turning_speed;
}

/*
 *  Stopping Methods
 */
void Mcs::Break()
{
    stopMotor0();
    stopMotor1();
    stopMotor2();
    stopMotor3();
}

void Mcs::stopMotor0()
{
    motor0Forward(0);
}

void Mcs::stopMotor1()
{
    motor1Forward(0);
}

void Mcs::stopMotor2()
{
    motor2Forward(0);
}

void Mcs::stopMotor3()
{
    motor3Forward(0);
}

/*
 *  Percent Methods
 */
void Mcs::leftMotorForward(int a){
    if(a < 0 || a > 100){
        motor0Coast();
        motor1Coast();
    }
    else{
        float helper = a/100;
        int speed_number = helper*255;
        motor0Forward(speed_number);
        motor1Forward(speed_number);
    }
}

void Mcs::leftMotorReverse(int a){
    if(a < 0 || a > 100){
        motor0Coast();
        motor1Coast();
    }
    else{
        float helper = a/100;
        int speed_number = helper*255;
        motor0Reverse(speed_number);
        motor1Reverse(speed_number);
    }
}

void Mcs::rightMotorForward(int a){
    if(a < 0 || a > 100){
        motor2Coast();
        motor3Coast();
    }
    else{
        float helper = a/100;
        int speed_number = helper*255;
        motor2Forward(speed_number);
        motor3Forward(speed_number);
    }
}

void Mcs::rightMotorReverse(int a){
    if(a < 0 || a > 100){
        motor2Coast();
        motor3Coast();
    }
    else{
        float helper = a/100;
        int speed_number = helper*255;
        motor2Reverse(speed_number);
        motor3Reverse(speed_number);
    }
}

uint8_t Mcs::getPWMParameter(int motor)
{
	switch(motor)
	{
	case 1:
		return LeftFrontSpeed;
		break;
	case 2:
		return LeftRearSpeed;
		break;
	case 3:
		return RightFrontSpeed;
		break;
	case 4:
		return RightRearSpeed;
		break;
	default:
		return 0;
	}
}

int Mcs::openSerialPort(){

	struct termios options1;

    int fd = open("/dev/ttySAC2", O_RDWR | O_NOCTTY | O_NDELAY);

	cout << "Opening serial port 1" << endl;
	tcgetattr(fd, &options1);
	cfsetispeed(&options1, B38400);
	cfsetospeed(&options1, B38400);
	options1.c_cflag = (options1.c_cflag & ~CSIZE) | CS8;
	options1.c_iflag = IGNBRK;
	options1.c_lflag = 0;
	options1.c_oflag = 0;
	options1.c_cflag |= CLOCAL | CREAD;
	options1.c_cc[VMIN] = 1;
	options1.c_cc[VTIME] = 5;
	//options1.c_iflag &= ~(IXON|IXOFF|IXANY);
	//options1.c_cflag &= ~(PARENB | PARODD);
	tcsetattr(fd,TCSANOW, &options1);

    return fd;
}

/*
 *    Private Helpers
 */
void Mcs::sendByte(uint8_t byteToSend)
{
    int n;
    unsigned char m[] = {byteToSend};
    //serialPort.write_to_port_Bytes(m,1);
    //fprintf(stderr, "Sending Byte: %x\n", m[0]);
    n=write(filePointer,m,1);
    if (n<0)
    Error("Unable to write");
}

void Mcs::readByte()
{
	int n;
	char readBuf[1024];
	cout << "Reading 1024 Bytes:" << endl;
	n = read(filePointer, readBuf, 1024);
	cout << n << endl;
	cout << (int)readBuf[0] << endl;
	cout << (int)readBuf[1] << endl;
}

void Mcs::Error(const char *Msg)
{
    fprintf (stderr, "%s\n", Msg);
    fprintf (stderr, "strerror() is %s\n", strerror(errno));
    exit(1);
}
void Mcs::Warning(const char *Msg)
{
     fprintf (stderr, "Warning: %s\n", Msg);
}

void Mcs::WaitSeconds(int seconds)
{
	time(&start);
	for(;;){
		time(&rightnow);
		diff = difftime(rightnow,start);
		if(diff >= seconds)
			break;
	}
}

//int main(void)
//{
//    int in1;
//
//    Mcs* mcs = new Mcs();
//
//    cout << "Select from the following:" << endl;
//    cout << "0: Configure Device" << endl;
//    cout << "1 or up: Run Tests" << endl;
//    cin >> in1;
//
//    if (in1 == 0)
//    {
//    	mcs -> sendByte(0xAA);
//    	mcs -> configMCs();
//    }
//    else
//    {
//		// Unit Test code
//		cout << "Testing Moving Forward 5 seconds at 50" << endl;
//		mcs -> MoveForwardAtSpeed(254);
//		mcs -> WaitSeconds(7);
////		mcs -> Stop();
//
////		mcs -> AdjustToStayOnLine(-50,0);
////		mcs -> WaitSeconds(1);
////
////		cout << "Testing Moving Forward 5 seconds at 50" << endl;
////		mcs -> MoveForwardAtSpeed(254);
////		mcs -> WaitSeconds(1);
////
////		mcs -> AdjustToStayOnLine(50,0);
////		mcs -> WaitSeconds(1);
////
////		cout << "Testing Moving Forward 5 seconds at 50" << endl;
////		mcs -> MoveForwardAtSpeed(254);
////		mcs -> WaitSeconds(1);
////
////		mcs -> AdjustToStayOnLine(0,40);
////		mcs -> WaitSeconds(1);
////
////		cout << "Testing Moving Forward 5 seconds at 50" << endl;
////		mcs -> MoveForwardAtSpeed(254);
////		mcs -> WaitSeconds(1);
////
////		mcs -> AdjustToStayOnLine(0,-40);
////		mcs -> WaitSeconds(1);
////
////		cout << "Testing Moving Forward 5 seconds at 50" << endl;
////		mcs -> MoveForwardAtSpeed(254);
////		mcs -> WaitSeconds(1);
////
////		mcs -> AdjustToStayOnLine(5,-40);
////		mcs -> WaitSeconds(1);
////
////		cout << "Testing Moving Forward 5 seconds at 50" << endl;
////		mcs -> MoveForwardAtSpeed(254);
////		mcs -> WaitSeconds(1);
////
////		mcs -> AdjustToStayOnLine(-5,40);
////		mcs -> WaitSeconds(1);
//
////		cout << "Testing Moving Forward 10 seconds at 255" << endl;
////		mcs -> MoveForwardAtSpeed(254);
////		mcs -> WaitSeconds(10);
////		mcs -> Stop();
//
////		cout << "Testing Moving Forward with speed at 300 (too great)" << endl;
////		mcs -> MoveForwardAtSpeed(300);
////		mcs -> WaitSeconds(2);
//
////		cout << "Testing Moving Backwards with speed at 100" << endl;
////		mcs -> MoveBackwardAtSpeed(200);
////		mcs -> WaitSeconds(5);
//
////		cout << "Testing Moving Backwards with speed at 300 (too great)" << endl;
////		mcs -> MoveBackwardAtSpeed(300);
////		mcs -> WaitSeconds(2);
//
////		cout << "Testing Stopping" << endl;
////		mcs -> Stop();
////		mcs -> WaitSeconds(1);
//
////		cout << "Testing Turning Left 90 deg" << endl;
////		mcs -> TurnLeft(90);
////		mcs -> WaitSeconds(2);
////
////		cout << "Testing Turning Right 90 deg" << endl;
////		mcs -> TurnRight(90);
////		mcs -> WaitSeconds(2);
//
////		cout << "Testing Turning Left 45 deg" << endl;
////		mcs -> TurnLeft(45);
////		mcs -> WaitSeconds(2);
//
////		cout << "Testing Moving Forward 10 seconds at 128" << endl;
////		mcs -> MoveForwardAtSpeed(200);
////		mcs -> WaitSeconds(10);
////		mcs -> Stop();
//
////		cout << "Testing Turning Right 45 deg" << endl;
////		mcs -> TurnRight(45);
////		mcs -> WaitSeconds(2);
//
////		cout << "Testing Turning Around (180 deg)" << endl;
////		mcs -> TurnLeft(180);
////		mcs -> WaitSeconds(1);
//
////		cout << "Testing Turning while in motion (5 deg, 0 off)" << endl;
////		mcs -> TurnLeft(180);
////		mcs -> WaitSeconds(2);
//
//    //Single MC commands
//    //    mcs -> sendByte(0x84);
//    //    mcs -> sendByte(1);
//    //    mcs -> sendByte(1);
//    //    mcs -> sendByte(0x55);
//    //    mcs -> sendByte(0x2A);
//
//      /*  while(1)
//        {
//        	cout << "Enter motor value" << endl;
//        	cin >> in1;
//    		if (in1 > 127)
//    		{
//    			mcs -> sendByte(0x89);
//    			mcs -> sendByte(in1-127);
//    		}
//    		else
//    		{
//    			mcs -> sendByte(0x88);
//    			mcs -> sendByte(in1);
//    		}
//        }
//        mcs -> sendByte(0x89);
//        mcs -> sendByte(0x7F);*/
//
//		cout << "Stopping" << endl;
//		mcs -> Stop();
//    }
//
//	cout << "All Done" << endl;
//    return 0;
//}
